//
// Created by 刘昱杉2020 on 2023/4/24.
//
#include "imu.h"
#include "usart.h"
#include <stdio.h>
#include "oled.h"
#define JY61P_PACKET_HEADER 0x55
#define JY61P_BUFFER_SIZE 11
#define JY61P_PACKET_SIZE 11
static uint8_t recv_buffer[JY61P_BUFFER_SIZE];
static uint16_t buffer_index = 0;
//uint8_t received_byte;
JY61P_Data data;


void JY61P_PrintData(const JY61P_Data *data);

void JY61P_ParseData(uint8_t *raw_data, JY61P_Data *imu_data) {
    imu_data->acc_x = (int16_t)(raw_data[1] << 8 | raw_data[0]) / 32768.0f * 16.0f;
    imu_data->acc_y = (int16_t)(raw_data[3] << 8 | raw_data[2]) / 32768.0f * 16.0f;
    imu_data->acc_z = (int16_t)(raw_data[5] << 8 | raw_data[4]) / 32768.0f * 16.0f;
    imu_data->gyro_x = (int16_t)(raw_data[7] << 8 | raw_data[6]) / 32768.0f * 2000.0f;
    imu_data->gyro_y = (int16_t)(raw_data[9] << 8 | raw_data[8]) / 32768.0f * 2000.0f;
    imu_data->gyro_z = (int16_t)(raw_data[11] << 8 | raw_data[10]) / 32768.0f * 2000.0f;
    imu_data->mag_x = (int16_t)(raw_data[13] << 8 | raw_data[12]) / 32768.0f * 1200.0f;
    imu_data->mag_y = (int16_t)(raw_data[15] << 8 | raw_data[14]) / 32768.0f * 1200.0f;
    imu_data->mag_z = (int16_t)(raw_data[17] << 8 | raw_data[16]) / 32768.0f * 1200.0f;
}
void JY61P_UART_BlockingRead(uint8_t *data, uint16_t size) {
    HAL_UART_Receive(&huart1, data, size, 1000);
}

void JY61P_BlockingUpdate(void) {
    uint8_t buffer[11];

    JY61P_UART_BlockingRead(buffer, 11);
    JY61P_ParseData(buffer, &data);
}

//
//uint8_t JY61P_ReadData(JY61P_Data *imu_data) {
//    uint8_t raw_data[22];
//    uint8_t recv_byte;
//    uint8_t idx = 0;
//
//    while (idx < 22) {
//        HAL_UART_Receive(&huart1, &recv_byte, 1, 10);
//
//        if (idx == 0 && recv_byte != JY61P_PACKET_HEADER) {
//            continue;
//        }
//
//        raw_data[idx] = recv_byte;
//        idx++;
//    }
//
//    if (raw_data[0] == JY61P_PACKET_HEADER && raw_data[11] == JY61P_PACKET_HEADER) {
//        JY61P_ParseData(raw_data, imu_data);
//        return 1;
//    } else {
//        return 0;
//    }
//}
//
//JY61P_Data imu_data;
void JY61P_UART_RxCpltCallback(void) {
    static uint8_t recv_buffer[JY61P_BUFFER_SIZE];
    static uint16_t buffer_index = 0;
    uint8_t received_byte;

    // 获取接收到的数据字节
    received_byte = (uint8_t)(huart1.Instance->DR & 0x00FF);

    // 检查数据包起始字节
    if (buffer_index == 0 && received_byte != JY61P_PACKET_HEADER) {
        return; // 不是数据包的起始字节，直接返回
    }

    // 将数据字节添加到接收缓冲区
    recv_buffer[buffer_index++] = received_byte;

    // 检查是否接收到完整的数据包
    if (buffer_index == JY61P_PACKET_SIZE) {
        buffer_index = 0; // 重置缓冲区索引，准备接收下一个数据包

        // 解析接收到的数据包
        JY61P_ParseData(recv_buffer,&data);
    }

    // 重新启用串口中断接收，准备接收下一个字节
    HAL_UART_Receive_IT(&huart1, &received_byte, 1);
    JY61P_PrintData(&data);
}
void JY61P_PrintData(const JY61P_Data *data) {
    OLED_ShowStr(1,2,1,"%.2f %.2f %.2f", data->acc_x, data->acc_y, data->acc_z);
    OLED_ShowStr(1,3,1,"%.2f %.2f %.2f", data->gyro_x, data->gyro_y, data->gyro_z);
    OLED_ShowStr(1,4,1,"%.2f %.2f %.2f",data->mag_x,data->mag_y,data->mag_z);
}
void USR_USART1_IRQHandler(void) {
    if (__HAL_UART_GET_FLAG(&huart1, UART_FLAG_RXNE) != RESET && __HAL_UART_GET_IT_SOURCE(&huart1, UART_IT_RXNE) != RESET) {
        __HAL_UART_CLEAR_PEFLAG(&huart1);
        JY61P_UART_RxCpltCallback();
    }
}
